import geomeas as gm
import numpy as np

# 相机光心坐标
Point_o = np.array([-94.64683716, 201.74600039, 2944.97607950])

# 三组激光光条投影点坐标
Point1_2 = np.array([-86.21081416, 197.69366833, 3151.89430970])
Point1_3 = np.array([-114.41114352, 197.69366833, 3151.89430970])

Point2_2 = np.array([-58.45949299, 197.25472245, 3151.89430970])
Point2_3 = np.array([-89.72410473, 197.25472245, 3151.89430970])

Point3_2 = np.array([-80.03299749, 196.83687861, 3151.89430970])
Point3_3 = np.array([-116.49078104, 196.83687861, 3151.89430970])

# 三组投影点坐标
PointP1_2 = np.array([-99.43781970, 197.69366833, 3151.84930970])

PointP2_2 = np.array([-66.13390250, 197.25472245, 3151.84930970])

PointP3_2 = np.array([-105.26292011, 196.83687861, 3151.84930970])

# 三组标定板坐标
Point1b_1 = np.array([-214.30534023, 459.22521645, 10])
Point1b_2 = np.array([185.69465977, 459.22521645, 10])
Point1b_3 = np.array([185.69465977, 59.22521645, 10])

Point2b_1 = np.array([-560.26597065, 359.62354155, 296.84342008])
Point2b_2 = np.array([-160.38939848, 369.55968899, 296.84342008])
Point2b_3 = np.array([-150.45325104, -30.31688319, 296.84342008])

Point3b_1 = np.array([-308.83206608, 374.81999449, 522.24212942])
Point3b_2 = np.array([65.98792884, 514.49809417, 522.24212942])
Point3b_3 = np.array([205.66602810, 139.67809926, 522.24212942])

# 激光光条上任意点坐标
P_1 = np.array([-214.30534023, 259.22521645, 10])

P_2 = np.array([-557.77126972, 259.22522682, 296.84342008])

P_3 = np.array([-265.75522843, 259.22520838, 522.24212942])

# 计算三个投影平面方程参数
a1, b1, c1, d1 = gm.Plane().calPlaneFrom3Points(Point_o, Point1_2, Point1_3)

a2, b2, c2, d2 = gm.Plane().calPlaneFrom3Points(Point_o, Point2_2, Point2_3)

a3, b3, c3, d3 = gm.Plane().calPlaneFrom3Points(Point_o, Point3_2, Point3_3)

Param1 = np.array([a1, b1, c1, d1])

Param2 = np.array([a2, b2, c2, d2])

Param3 = np.array([a3, b3, c3, d3])

# 计算三个标定板平面方程参数
ab1, bb1, cb1, db1 = gm.Plane().calPlaneFrom3Points(Point1b_1, Point1b_2, Point1b_3)

ab2, bb2, cb2, db2 = gm.Plane().calPlaneFrom3Points(Point2b_1, Point2b_2, Point2b_3)

ab3, bb3, cb3, db3 = gm.Plane().calPlaneFrom3Points(Point3b_1, Point3b_2, Point3b_3)

Paramb1 = np.array([ab1, bb1, cb1, db1])

Paramb2 = np.array([ab2, bb2, cb2, db2])

Paramb3 = np.array([ab3, bb3, cb3, db3])

# 计算三个投影直线的方向向量
m1, n1, p1 = gm.Vector().calVectorFrom2Points(Point_o, PointP1_2)

m2, n2, p2 = gm.Vector().calVectorFrom2Points(Point_o, PointP2_2)

m3, n3, p3 = gm.Vector().calVectorFrom2Points(Point_o, PointP3_2)

ProjectionVector_1 = np.array([m1, n1, p1])

ProjectionVector_2 = np.array([m2, n2, p2])

ProjectionVector_3 = np.array([m3, n3, p3])

# 计算投影平面与标定板平面的交线方向向量
m_1, n_1, p_1 = gm.Vector().calVectorFrom2Planes(Param1, Paramb1)

m_2, n_2, p_2 = gm.Vector().calVectorFrom2Planes(Param2, Paramb2)

m_3, n_3, p_3 = gm.Vector().calVectorFrom2Planes(Param3, Paramb3)

LaserVector_1 = np.array([m_1, n_1, p_1])

LaserVector_2 = np.array([m_2, n_2, p_2])

LaserVector_3 = np.array([m_3, n_3, p_3])

# 计算投影直线与激光光条的交点
x1, y1, z1 = gm.Coordinate().calCoordinateFrom2Lines(P_1, LaserVector_1, Point_o, ProjectionVector_1)

x2, y2, z2 = gm.Coordinate().calCoordinateFrom2Lines(P_2, LaserVector_2, Point_o, ProjectionVector_2)

x3, y3, z3 = gm.Coordinate().calCoordinateFrom2Lines(P_3, LaserVector_3, Point_o, ProjectionVector_3)

PointPlane1 = np.array([x1, y1, z1])

PointPlane2 = np.array([x2, y2, z2])

PointPlane3 = np.array([x3, y3, z3])

# 利用三点计算得到光平面方程参数
a, b, c, d = gm.Plane().calPlaneFrom3Points(PointPlane1, PointPlane2, PointPlane3)
LightPlaneParam = np.array([a, b, c, d]).transpose()

print('LightPlaneParam: ', LightPlaneParam)

